﻿#region netDxf library licensed under the MIT License
// 
//                       netDxf library
// Copyright (c) Daniel Carvajal (haplokuon@gmail.com)
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in all
// copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
// 
#endregion

// This is a translation to C# from the original C++ code of the Geometric Tool Library
// Original license
// David Eberly, Geometric Tools, Redmond WA 98052
// Copyright (c) 1998-2022
// Distributed under the Boost Software License, Version 1.0.
// https://www.boost.org/LICENSE_1_0.txt
// https://www.geometrictools.com/License/Boost/LICENSE_1_0.txt
// Version: 6.0.2022.01.06

using System;
using System.Diagnostics;

namespace netDxf.GTE
{
    public class GVector :
        IEquatable<GVector>
    {
        private readonly double[] vector;

        //// The tuple is length zero (uninitialized).
        //public GVector()
        //{
        //    this.vector = new double[0];
        //}

        // The tuple is length 'size' and the elements are uninitialized.
        public GVector(int size)
        {
            this.vector = new double[size];
        }

        // For 0 <= d <= size, element d is 1 and all others are zero.  If d
        // is invalid, the zero vector is created.  This is a convenience for
        // creating the standard Euclidean basis vectors; see also
        // MakeUnit(int,int) and Unit(int,int).
        public GVector(int size, int d)
        {
            this.vector = new double[size];
            this.MakeUnit(d);
        }

        public GVector(double[] elements)
        {
            this.vector = new double[elements.Length];
            elements.CopyTo(this.vector, 0);
        }

        // The copy constructor, destructor, and assignment operator are
        // generated by the compiler.

        // Member access.  SetSize(int) does not initialize the tuple.  The
        // first operator[] returns a const reference rather than a double
        // value.  This supports writing via standard file operations that
        // require a const pointer to data.
        //public void SetSize(int size)
        //{
        //    Debug.Assert(size >= 0, "Invalid size.");
        //    this.vector = new double[size];
        //}

        public int Size
        {
            get { return this.vector.Length; }
        }

        public double this[int i]
        {
            get { return this.vector[i]; }
            set { this.vector[i] = value; }
        }

        public double[] Vector
        {
            get { return this.vector; }
        }

        // Comparison (for use by STL containers).
        public static bool operator ==(GVector vec1, GVector vec2)
        {
            if (vec1 == null || vec2 == null)
            {
                return false;
            }
            return vec1.Equals(vec2);
        }

        public static bool operator !=(GVector vec1, GVector vec2)
        {
            if (vec1 == null || vec2 == null)
            {
                return false;
            }
            return !vec1.Equals(vec2);
        }

        public static bool operator <(GVector vec1, GVector vec2)
        {
            if (vec1.Size != vec2.Size)
            {
                return false;
            }

            int size = vec1.Size;
            for (int i = 0; i < size; i++)
            {
                if (vec1[i] < vec2[i])
                {
                    return false;
                }
            }

            return true;
        }

        public static bool operator <=(GVector vec1, GVector vec2)
        {
            if (vec1.Size != vec2.Size)
            {
                return false;
            }

            int size = vec1.Size;
            for (int i = 0; i <= size; i++)
            {
                if (vec1[i] < vec2[i])
                {
                    return false;
                }
            }

            return true;
        }

        public static bool operator >(GVector vec1, GVector vec2)
        {
            if (vec1.Size != vec2.Size)
            {
                return false;
            }

            int size = vec1.Size;
            for (int i = 0; i < size; i++)
            {
                if (vec1[i] > vec2[i])
                {
                    return false;
                }
            }

            return true;
        }

        public static bool operator >=(GVector vec1, GVector vec2)
        {
            if (vec1.Size != vec2.Size)
            {
                return false;
            }

            int size = vec1.Size;
            for (int i = 0; i < size; i++)
            {
                if (vec1[i] >= vec2[i])
                {
                    return false;
                }
            }

            return true;
        }

        // Special vectors.

        // All components are 0.
        public void MakeZero()
        {
            for (int i = 0; i < this.vector.Length; i++)
            {
                this.vector[i] = 0.0;
            }
        }

        // Component d is 1, all others are zero.
        public void MakeUnit(int d)
        {
            for (int i = 0; i < this.vector.Length; i++)
            {
                if (i == d)
                {
                    this.vector[i] = 1.0;
                }
                else
                {
                    this.vector[i] = 0.0;
                }
            }
        }

        public static GVector Zero(int size)
        {
            return new GVector(size);
        }

        public static GVector Unit(int size, int d)
        {
            return new GVector(size, d);
        }


        // Unary operations.

        // Linear-algebraic operations.
        public static GVector operator +(GVector vec1, GVector vec2)
        {
            if (vec1 == null || vec2 == null)
            {
                return null;
            }

            if (vec1.Size == vec2.Size)
            {
                int size = vec1.Size;
                GVector vector = new GVector(size);
                for (int i = 0; i < size; i++)
                {
                    vector[i] = vec1[i] + vec2[i];
                }

                return vector;
            }

            Debug.Assert(false, "Mismatched sizes.");
            return null;
        }

        public static GVector operator -(GVector vec1, GVector vec2)
        {
            if (vec1 == null || vec2 == null)
            {
                return null;
            }

            if (vec1.Size == vec2.Size)
            {
                int size = vec1.Size;
                GVector vector = new GVector(size);
                for (int i = 0; i < size; i++)
                {
                    vector[i] = vec1[i] - vec2[i];
                }

                return vector;
            }

            Debug.Assert(false, "Mismatched sizes.");
            return null;
        }

        public static GVector operator *(double scalar, GVector vec)
        {
            if (vec == null)
            {
                return null;
            }

            int size = vec.Size;
            GVector vector = new GVector(size);
            for (int i = 0; i < size; i++)
            {
                vector[i] = scalar * vec[i];
            }

            return vector;
        }

        public static GVector operator *(GVector vec, double scalar)
        {
            return scalar * vec;
        }

        public static GVector operator /(GVector vec, double scalar)
        {
            return vec * (1.0 / scalar);
        }

        // Geometric operations.  The functions with 'robust' set to 'false' use
        // the standard algorithm for normalizing a vector by computing the length
        // as a square root of the squared length and dividing by it.  The results
        // can be infinite (or NaN) if the length is zero.  When 'robust' is set
        // to 'true', the algorithm is designed to avoid floating-point overflow
        // and sets the normalized vector to zero when the length is zero.
        public static double Dot(GVector v0, GVector v1)
        {
            if (v0 == null || v1 == null)
            {
                return double.NaN;
            }

            if (v0.Size == v1.Size)
            {
                double dot = 0.0;
                for (int i = 0; i < v0.Size; i++)
                {
                    dot += v0[i] * v1[i];
                }

                return dot;
            }

            Debug.Assert(false, "Mismatched sizes.");
            return double.NaN;
        }

        public static double Length(GVector v, bool robust = false)
        {
            if (v == null)
            {
                return double.NaN;
            }

            if (robust)
            {
                double maxAbsComp = Math.Abs(v[0]);
                for (int i = 1; i < v.Size; ++i)
                {
                    double absComp = Math.Abs(v[i]);
                    if (absComp > maxAbsComp)
                    {
                        maxAbsComp = absComp;
                    }
                }

                double length;
                if (maxAbsComp > 0.0)
                {
                    GVector scaled = v / maxAbsComp;
                    length = maxAbsComp * Math.Sqrt(Dot(scaled, scaled));
                }
                else
                {
                    length = 0.0;
                }

                return length;
            }

            return Math.Sqrt(Dot(v, v));
        }

        public static double Normalize(ref GVector v, bool robust = false)
        {
            if (v == null)
            {
                return double.NaN;
            }

            if (robust)
            {
                double maxAbsComp = Math.Abs(v[0]);
                for (int i = 1; i < v.Size; i++)
                {
                    double absComp = Math.Abs(v[i]);
                    if (absComp > maxAbsComp)
                    {
                        maxAbsComp = absComp;
                    }
                }

                double length;
                if (maxAbsComp > 0.0)
                {
                    v /= maxAbsComp;
                    length = Math.Abs(Dot(v, v));
                    v /= length;
                    length *= maxAbsComp;
                }
                else
                {
                    length = 0.0;
                    for (int i = 0; i < v.Size; i++)
                    {
                        v[i] = 0.0;
                    }
                }

                return length;
            }
            else
            {
                double length = Math.Sqrt(Dot(v, v));
                if (length > 0.0)
                {
                    v /= length;
                }
                else
                {
                    for (int i = 0; i < v.Size; i++)
                    {
                        v[i] = 0.0;
                    }
                }

                return length;
            }
        }

        // Gram-Schmidt orthonormalization to generate orthonormal vectors from
        // the linearly independent inputs.  The function returns the smallest
        // length of the unnormalized vectors computed during the process.  If
        // this value is nearly zero, it is possible that the inputs are linearly
        // dependent (within numerical round-off errors).  On input,
        // 1 <= numElements <= N and v[0] through v[numElements-1] must be
        // initialized.  On output, the vectors v[0] through v[numElements-1]
        // form an orthonormal set.
        public static double Orthonormalize(int numInputs, ref GVector[] v, bool robust = false)
        {
            if (v != null && 1 <= numInputs && numInputs <= v[0].Size)
            {
                for (int i = 1; i < numInputs; ++i)
                {
                    if (v[0].Size != v[i].Size)
                    {
                        Debug.Assert(false, "Mismatched sizes.");
                    }
                }

                double minLength = Normalize(ref v[0], robust);
                for (int i = 1; i < numInputs; ++i)
                {
                    for (int j = 0; j < i; ++j)
                    {
                        double dot = Dot(v[i], v[j]);
                        v[i] -= v[j] * dot;
                    }

                    double length = Normalize(ref v[i], robust);
                    if (length < minLength)
                    {
                        minLength = length;
                    }
                }

                return minLength;
            }

            Debug.Assert(false, "Invalid input.");
            return double.NaN;
        }

        // Compute the axis-aligned bounding box of the vectors.  The return value is
        // 'true' iff the inputs are valid, in which case vmin and vmax have valid
        // values.
        public static bool ComputeExtremes(int numVectors, GVector[] v, out GVector vmin, out GVector vmax)
        {
            if (v != null && numVectors > 0)
            {
                for (int i = 1; i < numVectors; ++i)
                {
                    if (v[0].Size != v[i].Size)
                    {
                        Debug.Assert(false, "Mismatched sizes.");
                    }
                }

                int size  = v[0].Size;
                vmin = v[0];
                vmax = vmin;
                for (int j = 1; j < numVectors; ++j)
                {
                    GVector  vec = v[j];
                    for (int i = 0; i < size; ++i)
                    {
                        if (vec[i] < vmin[i])
                        {
                            vmin[i] = vec[i];
                        }
                        else if (vec[i] > vmax[i])
                        {
                            vmax[i] = vec[i];
                        }
                    }
                }

                return true;
            }

            vmin = null;
            vmax = null;
            Debug.Assert(false, "Invalid input.");
            return false;
        }

        // Lift n-tuple v to homogeneous (n+1)-tuple (v,last).
        public GVector HLift(GVector v, double last)
        {
            if (v == null)
            {
                return null;
            }

            int size  = v.Size;
            GVector result = new GVector(size + 1);
            for (int i = 0; i < size; i++)
            {
                result[i] = v[i];
            }

            result[size] = last;
            return result;
        }

        // Project homogeneous n-tuple v = (u,v[n-1]) to (n-1)-tuple u.
        public GVector HProject(GVector v)
        {
            if (v == null)
            {
                return null;
            }

            int size = v.Size;
            if (size > 1)
            {
                GVector result = new GVector(size - 1);
                for (int i = 0; i < size - 1; i++)
                {
                    result[i] = v[i];
                }

                return result;
            }

            return null;
        }


        // Lift n-tuple v = (w0,w1) to (n+1)-tuple u = (w0,u[inject],w1).  By
        // inference, w0 is a (inject)-tuple [nonexistent when inject=0] and w1
        // is a (n-inject)-tuple [nonexistent when inject=n].
        public GVector Lift(GVector v, int inject, double value)
        {
            int size  = v.Size;
            GVector result = new GVector(size + 1);
            int i;
            for (i = 0; i < inject; ++i)
            {
                result[i] = v[i];
            }

            result[i] = value;
            int j = i;
            for (++j; i < size; ++i, ++j)
            {
                result[j] = v[i];
            }

            return result;
        }

        // Project n-tuple v = (w0,v[reject],w1) to (n-1)-tuple u = (w0,w1).  By
        // inference, w0 is a (reject)-tuple [nonexistent when reject=0] and w1
        // is a (n-1-reject)-tuple [nonexistent when reject=n-1].
        public GVector Project(GVector v, int reject)
        {
            int size  = v.Size;
            if (size > 1)
            {
                GVector result = new GVector(size - 1);
                for (int i = 0, j = 0; i < size - 1; ++i, ++j)
                {
                    if (j == reject)
                    {
                        ++j;
                    }

                    result[i] = v[j];
                }

                return result;
            }

            return new GVector(0);
        }

        public bool Equals(GVector other)
        {
            if (other == null)
            {
                return false;
            }

            if (this.Size != other.Size)
            {
                return false;
            }

            int size = this.Size;
            for (int i = 0; i < size; i++)
            {
                if (Math.Abs(this[i] - other[i]) > double.Epsilon)
                {
                    return false;
                }
            }

            return true;
        }

        public override bool Equals(object obj)
        {
            if (obj == null)
            {
                return false;
            }

            return obj.GetType() == this.GetType() && this.Equals((GVector) obj);
        }

        public override int GetHashCode()
        {
            return this.vector.GetHashCode();
        }
    }
}
